/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : app_freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "arm_math.h"

#include "tim.h"
#include "adc.h"
#include "opamp.h"
#include "usart.h"

#include "bsp.h"
#include "motor.h"
#include "button.h"
#include "lowpass_filter.h"
#include "foc.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */



/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
#define CH_COUNT 12
struct Frame
{
    float fdata[CH_COUNT];
    unsigned char tail[4];
};

struct Frame vfoaFrame = {
        .tail = {0x00, 0x00, 0x80, 0x7f}
};


float gIdRef = 0.0f;
float gIqRef = 0.1f;
float gSpeedRef = 10.0f;
float gPosRef = 0.5f;
float gDetTheta = 0.005f;
bool uartIdle;
uint32_t vofaCmdLen = 0;
uint8_t vofaRxItBuf[1];
#define UART3_BUF_MAX_SIZE   4096
uint8_t uart3Buf[UART3_BUF_MAX_SIZE];
uint8_t vofaCmdBuf[1024];


/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
        .name = "defaultTask",
        .priority = (osPriority_t) osPriorityLow,
        .stack_size = 256 * 4
};
/* Definitions for gBtnScanTask */
osThreadId_t gBtnScanTaskHandle;
const osThreadAttr_t gBtnScanTask_attributes = {
        .name = "gBtnScanTask",
        .priority = (osPriority_t) osPriorityNormal,
        .stack_size = 256 * 4
};
/* Definitions for gGetAngleTask */
osThreadId_t gGetAngleTaskHandle;
const osThreadAttr_t gGetAngleTask_attributes = {
        .name = "gGetAngleTask",
        .priority = (osPriority_t) osPriorityHigh,
        .stack_size = 256 * 4
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

static void bsp_btn_event_callback(void *btn, int event);

/**
 * @brief Control motor work status
 * @param btn
 * @param event
 */
static void bsp_btn_event_callback(void *btn, int event)
{
    if (btn == &bsp_btn1)
    {
    }
    else if (btn == &bsp_btn2)
    {
    }
    else if (btn == &bsp_btn3)
    {
        if (event == ButtonEvent_SingleClick)
        {
            motor1.status = motor1.status == MOTOR_STATUS_RUN ? MOTOR_STATUS_STOP : MOTOR_STATUS_RUN;

            if (motor1.status == MOTOR_STATUS_RUN)
            {
                motor_enable(&motor1);
            }
            else
            {
                motor_disable(&motor1);
            }
        }
    }
    else
    {
    }

    HAL_GPIO_TogglePin(LED_STATUS_GPIO_Port, LED_STATUS_Pin);
}


int pulse;
float THETA;

void pulse_to_THETA(int a, int b, int c, int d, int e, int f, int g)
{
    if (pulse > a && pulse <= b)
    {
        THETA = (float) (pulse - a) / (b - a) * (float) M_TWOPI;
    }
    if (pulse > b && pulse <= c)
    {
        THETA = (float) (pulse - b) / (c - b) * (float) M_TWOPI;
    }
    if (pulse > c && pulse <= d)
    {
        THETA = (float) (pulse - c) / (d - c) * (float) M_TWOPI;
    }
    if (pulse > d && pulse <= e)
    {
        THETA = (float) (pulse - d) / (e - d) * (float) M_TWOPI;
    }
    if (pulse > e && pulse <= f)
    {
        THETA = (float) (pulse - e) / (f - e) * (float) M_TWOPI;
    }
    if (pulse > f && pulse <= g)
    {
        THETA = (float) (pulse - f) / (g - f) * (float) M_TWOPI;
    }
    if (pulse > g && pulse <= 16384)
    {
        THETA = (float) (pulse - g) / (16384 + a - g) * (float) M_TWOPI;
    }
    if (pulse > 0 && pulse <= a)
    {
        THETA = (float) (pulse + 16384 - g) / (16384 + a - g) * (float) M_TWOPI;
    }
}


#define ENCODER_CPR 16384

long GetCurrentAbsTotalValue(long lValue)
{
    long m_absEncTotal;           // 计算编码器�?�时间片变化�?????
    static int absEnc_Q_count;     // 单圈绝对值编码器圈数计算
    static int data_count = 0;       // 计一次运行改�?????1，用于数据缓存初始化
    static int m_absEncBuff[5] = {0}; // 绝对值编码器数据缓存

    m_absEncBuff[0] = lValue; //  读取数据缓存

    if (data_count < 1)
    {//�?????机初始化
        data_count++;
        m_absEncBuff[1] = m_absEncBuff[2] = m_absEncBuff[3] = m_absEncBuff[0];
        absEnc_Q_count = 1000;
    }

    if (abs(m_absEncBuff[0] - m_absEncBuff[1]) > (ENCODER_CPR + 500))
    {// 去除异常数据
        m_absEncBuff[0] = m_absEncBuff[1] + (m_absEncBuff[1] - m_absEncBuff[2]);
    }


    if ((m_absEncBuff[0] - m_absEncBuff[1]) > ENCODER_CPR - 1000)
    {// 提升或圆盘顺时针过程,数�?�减�?????
        absEnc_Q_count--;
    }
    else if ((m_absEncBuff[1] - m_absEncBuff[0]) > ENCODER_CPR - 1000)
    {// 下降或圆盘�?�时针过�?????,数�?�增�?????
        absEnc_Q_count++;
    }

    m_absEncTotal = absEnc_Q_count * ENCODER_CPR + m_absEncBuff[0];

    m_absEncBuff[3] = m_absEncBuff[2];
    m_absEncBuff[2] = m_absEncBuff[1];
    m_absEncBuff[1] = m_absEncBuff[0];
    //TRACE("%d-- %d -- %d\n", lValue, absEnc_Q_count, m_absEncTotal);
    return (m_absEncTotal);
}


#define WIN_BUF_SIZE    64
float windowFilterBuf[WIN_BUF_SIZE];

float window_filter(float data, float *buf, int len)
{
    float sum = 0;

    for (int i = 1; i < len; i++)
    {
        buf[i - 1] = buf[i];
    }
    buf[len - 1] = data;

    for (int i = 0; i < len; i++)
    {
        sum += buf[i];
    }
    sum /= len;

    return sum;
}

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);

void StartBtnScanTask(void *argument);

void StartGetAngleTask(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void)
{
    /* USER CODE BEGIN Init */

    bsp_init();
    HAL_Delay(200);

    button_install_event_callback(&bsp_btn1, ButtonEvent_SingleClick, bsp_btn_event_callback);
    button_install_event_callback(&bsp_btn2, ButtonEvent_SingleClick, bsp_btn_event_callback);
    button_install_event_callback(&bsp_btn3, ButtonEvent_SingleClick, bsp_btn_event_callback);

    /* Calibrate encoder electricity zero position */
    motor_set_status(&motor1, MOTOR_STATUS_STOP);
//    motor_align_encoder(&motor1, 1.5f, 0.0f);


    /* vofa+ debug */
    HAL_UARTEx_ReceiveToIdle_DMA(&huart3, vofaCmdBuf, 1024);


    /* USER CODE END Init */

    /* USER CODE BEGIN RTOS_MUTEX */
    /* add mutexes, ... */
    /* USER CODE END RTOS_MUTEX */

    /* USER CODE BEGIN RTOS_SEMAPHORES */
    /* add semaphores, ... */
    /* USER CODE END RTOS_SEMAPHORES */

    /* USER CODE BEGIN RTOS_TIMERS */
    /* start timers, add new ones, ... */
    /* USER CODE END RTOS_TIMERS */

    /* USER CODE BEGIN RTOS_QUEUES */
    /* add queues, ... */
    /* USER CODE END RTOS_QUEUES */

    /* Create the thread(s) */
    /* creation of defaultTask */
    defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

    /* creation of gBtnScanTask */
    gBtnScanTaskHandle = osThreadNew(StartBtnScanTask, NULL, &gBtnScanTask_attributes);

    /* creation of gGetAngleTask */
    gGetAngleTaskHandle = osThreadNew(StartGetAngleTask, NULL, &gGetAngleTask_attributes);

    /* USER CODE BEGIN RTOS_THREADS */
    /* add threads, ... */
    /* USER CODE END RTOS_THREADS */

    /* USER CODE BEGIN RTOS_EVENTS */
    /* add events, ... */
    /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
    /* USER CODE BEGIN StartDefaultTask */
    const float udcGain = 3.3f / 4096 * 26;

    /* Infinite loop */
    for (;;)
    {
        HAL_GPIO_TogglePin(LED_BLINK_GPIO_Port, LED_BLINK_Pin);

        HAL_ADC_Start(&hadc2);
        motor1.udc = (float32_t) HAL_ADC_GetValue(&hadc2) * udcGain;
        HAL_Delay(10);

        osDelay(1);
    }
    /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_StartBtnScanTask */

/**
* @brief Function implementing the gBtnScanTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartBtnScanTask */
void StartBtnScanTask(void *argument)
{
    /* USER CODE BEGIN StartBtnScanTask */

    /* Infinite loop */
    for (;;)
    {
        bsp_btn_ticks();
        HAL_Delay(5);

        osDelay(1);
    }
    /* USER CODE END StartBtnScanTask */
}

/* USER CODE BEGIN Header_StartGetAngleTask */
/**
* @brief Function implementing the gGetAngleTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartGetAngleTask */
void StartGetAngleTask(void *argument)
{
    /* USER CODE BEGIN StartGetAngleTask */
    static uint32_t lastTick;
    uint32_t tick = 0;
    uint32_t detTick = 0;

    /* Infinite loop */
    for (;;)
    {
        uint16_t raw = 0;
//        vfoaFrame.fdata[4] = (float)mt6701_get_raw_data(&bsp_mt6701, &raw);

//        motor_get_angle_rad(&motor1);

        osDelay(1);
    }
    /* USER CODE END StartGetAngleTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */


/**
  * @brief  Period elapsed callback in non-blocking mode
  * @param  htim TIM handle
  * @retval None
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    static float sLastAngleRad = 0;
    float detAngleRad;

    if (htim->Instance == TIM17)
    {
        HAL_IncTick();
    }

    if (!true && htim->Instance == TIM3)
    {
        detAngleRad = (float) GetCurrentAbsTotalValue((long) motor1.encoderRawData) - sLastAngleRad;
        detAngleRad = window_filter(detAngleRad, windowFilterBuf, WIN_BUF_SIZE);

        motor1.speedRpm = ((detAngleRad / ENCODER_CPR) / (float) M_TWOPI) / (0.5e-4f);
        motor1.speedRpm *= 60.0f;

        sLastAngleRad = (float) GetCurrentAbsTotalValue((long) motor1.encoderRawData);
    }
}


/**
  * @brief  Injected conversion complete callback in non-blocking mode.
  * @param hadc ADC handle
  * @retval None
  */
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{
    static float sLastAngleRad = 0;
    static int sLastEncoderDataRaw = 0;
    static uint32_t sLastTick = 0;
    uint32_t tick;
    float detAngleRad;
    uint32_t detTick;

    /* Calc ADC offset */
    static bool calcAdcOffsetOvered = false;
    static uint32_t iaOffset = 0, ibOffset = 0, icOffset = 0;
    const int measCnt = 20;
    static int measCntCopy = measCnt;
    if (hadc->Instance == ADC1 && !calcAdcOffsetOvered)
    {
        iaOffset += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);
        icOffset += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2);
        //bsp_board.ibOffset += HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);

        if (--measCntCopy <= 0)
        {
            bsp_board.iaOffset = iaOffset / measCnt;
            bsp_board.icOffset = icOffset / measCnt;
            //bsp_board.ibOffset /= measCnt;

            calcAdcOffsetOvered = true;
        }
    }

    /* Current closed loop */
    if (hadc->Instance == ADC1 && calcAdcOffsetOvered)
    {
        HAL_GPIO_WritePin(FREQ_TEST_GPIO_Port, FREQ_TEST_Pin, GPIO_PIN_SET);

        /* Get phase current. */
        bsp_get_phase_current(&bsp_board, &motor1.phaseCurrent.ia, &motor1.phaseCurrent.ib, &motor1.phaseCurrent.ic);

        /* Get motor electricity angle. */
        motor_get_elec_angle(&motor1);

        /* Calc speed */
        if (true)
        {
            detAngleRad = (float) GetCurrentAbsTotalValue((long) motor1.encoderRawData) - sLastAngleRad;
            motor1.speedRpm = ((detAngleRad / ENCODER_CPR) / (float) M_TWOPI) / (0.5e-4f);
            motor1.speedRpm *= 60.0f;
            motor1.speedRpm = window_filter(motor1.speedRpm, windowFilterBuf, WIN_BUF_SIZE);

            sLastAngleRad = (float) GetCurrentAbsTotalValue((long) motor1.encoderRawData);
        }

        if (!true)
        {
            motor1.theta += 0.003f;
            if (motor1.theta > M_TWOPI)
                motor1.theta -= M_TWOPI;
            motor1.sincosTheta.sinTheta = arm_sin_f32(motor1.theta);
            motor1.sincosTheta.cosTheta = arm_cos_f32(motor1.theta);
        }

        if (motor1.status == MOTOR_STATUS_RUN)
        {
//            motor_open_loop_test(&motor1, 0.0f, 1.0f);
            motor_current_closed_loop(&motor1, gIdRef, gIqRef);
        }

        /* vofa debug */
        vfoaFrame.fdata[0] = motor1.phaseCurrent.ia;
        vfoaFrame.fdata[1] = motor1.phaseCurrent.ib;
        vfoaFrame.fdata[2] = motor1.encoderRawData;
        vfoaFrame.fdata[3] = motor1.theta;
        if (false)
        {
            vfoaFrame.fdata[3] = (float) motor1.alphabetaCurrent.ialpha;
            vfoaFrame.fdata[4] = (float) motor1.alphabetaCurrent.ibeta;
        }
        if (false)
        {
            vfoaFrame.fdata[2] = (float) motor1.dqCurrent.id;
            vfoaFrame.fdata[3] = (float) motor1.dqCurrent.iq;
        }
        if (false)
        {
            vfoaFrame.fdata[2] = (float) motor1.dqVoltage.ud;
            vfoaFrame.fdata[3] = (float) motor1.dqVoltage.uq;
        }
        if (false)
        {
            vfoaFrame.fdata[4] = (float) motor1.pwmParam.ccra;
            vfoaFrame.fdata[5] = (float) motor1.pwmParam.ccrb;
            vfoaFrame.fdata[6] = (float) motor1.pwmParam.ccrc;
        }
        if (false)
        {
            vfoaFrame.fdata[0] = motor1.dCurrentPid.kp;
            vfoaFrame.fdata[1] = motor1.dCurrentPid.ki;
            vfoaFrame.fdata[2] = motor1.dqCurrent.id;
            vfoaFrame.fdata[3] = gIdRef;
        }
        if (false)
        {
            vfoaFrame.fdata[4] = motor1.qCurrentPid.kp;
            vfoaFrame.fdata[5] = motor1.qCurrentPid.ki;
            vfoaFrame.fdata[6] = motor1.dqCurrent.iq;
            vfoaFrame.fdata[7] = gIqRef;
        }
        if (!false)
        {
            vfoaFrame.fdata[2] = motor1.speedPid.kp;
            vfoaFrame.fdata[3] = motor1.speedPid.ki;
            vfoaFrame.fdata[4] = gSpeedRef;
            vfoaFrame.fdata[5] = motor1.speedRpm;
            vfoaFrame.fdata[6] = gIqRef;
            vfoaFrame.fdata[7] = motor1.dqCurrent.iq;
        }
        if (!false)
        {
            vfoaFrame.fdata[8] = motor1.positionPid.kp;
            vfoaFrame.fdata[9] = motor1.positionPid.ki;
            vfoaFrame.fdata[10] = gPosRef;
            vfoaFrame.fdata[11] = motor1.angleRad;
            vfoaFrame.fdata[4] = gSpeedRef;
            vfoaFrame.fdata[5] = motor1.speedRpm;
        }
        HAL_UART_Transmit_DMA(&huart3, (uint8_t *) (&vfoaFrame), sizeof(vfoaFrame));
        HAL_GPIO_WritePin(FREQ_TEST_GPIO_Port, FREQ_TEST_Pin, GPIO_PIN_RESET);
    }

    /* Speed closed loop */
    static uint32_t speedFbkCnt = 0;
    if (motor1.status == MOTOR_STATUS_RUN && ++speedFbkCnt >= 4 && !false)
    {
        speedFbkCnt = 0; // reset counter

        motor_speed_closed_loop(&motor1, gSpeedRef);
        gIqRef = motor1.speedPid.output;

        motor_position_closed_loop(&motor1, gPosRef);
        gSpeedRef = motor1.positionPid.output;
    }

    /* Position closed loop */
    static uint32_t posFbkCnt = 0;
    if (motor1.status == MOTOR_STATUS_RUN && ++posFbkCnt >= 10 && false)
    {
        posFbkCnt = 0; // reset counter

        motor_position_closed_loop(&motor1, gPosRef);
        gSpeedRef = motor1.positionPid.output;
    }
}


/**
 * @brief Parse vofa cmd
 * @param cmdBuf
 * @return
 */
static float vofa_cmd_parse(uint8_t *cmdBuf, char *arg)
{
    return atof(cmdBuf + strlen(arg));
}

/**
  * @brief  Reception Event Callback (Rx event notification called after use of advanced reception service).
  * @param  huart UART handle
  * @param  Size  Number of data available in application reception buffer (indicates a position in
  *               reception buffer until which, data are available)
  * @retval None
  */
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
    if (huart->Instance == USART3)
    {
        if (strstr(vofaCmdBuf, "uq"))
        {
            motor1.dqVoltage.uq = vofa_cmd_parse(vofaCmdBuf, "uq");
        }
        else if (strstr(vofaCmdBuf, "idkp="))
        {
            float kp = vofa_cmd_parse(vofaCmdBuf, "idkp=");
            motor1.dCurrentPid.kp = kp;
        }
        else if (strstr(vofaCmdBuf, "idki="))
        {
            float ki = vofa_cmd_parse(vofaCmdBuf, "idkp=");
            motor1.dCurrentPid.ki = ki;
        }
        else if (strstr(vofaCmdBuf, "iqkp="))
        {
            float kp = vofa_cmd_parse(vofaCmdBuf, "iqkp=");
            motor1.qCurrentPid.kp = kp;
        }
        else if (strstr(vofaCmdBuf, "iqki="))
        {
            float ki = vofa_cmd_parse(vofaCmdBuf, "iqki=");
            motor1.qCurrentPid.ki = ki;
        }
        else if (strstr(vofaCmdBuf, "iqref="))
        {
            float iqref = vofa_cmd_parse(vofaCmdBuf, "iqref=");
            gIqRef = iqref;
        }
        else if (strstr(vofaCmdBuf, "idref="))
        {
            float idref = vofa_cmd_parse(vofaCmdBuf, "idref=");
            gIdRef = idref;
        }
        else if (strstr(vofaCmdBuf, "radOffset="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "radOffset=");
            motor1.angleRadOffset = val;
        }
        else if (strstr(vofaCmdBuf, "velref="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "velref=");
            gSpeedRef = val;
        }
        else if (strstr(vofaCmdBuf, "velkp="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "velkp=");
            motor1.speedPid.kp = val;
        }
        else if (strstr(vofaCmdBuf, "velki="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "velki=");
            motor1.speedPid.ki = val;
        }
        else if (strstr(vofaCmdBuf, "pos_ref="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "pos_ref=");
            gPosRef = val;
        }
        else if (strstr(vofaCmdBuf, "pos_kp="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "pos_kp=");
            motor1.positionPid.kp = val;
        }
        else if (strstr(vofaCmdBuf, "pos_ki="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "pos_ki=");
            motor1.positionPid.ki = val;
        }
        else
        {
        }

        memset(vofaCmdBuf, 0, 1024);
        HAL_UARTEx_ReceiveToIdle_DMA(&huart3, vofaCmdBuf, 1024);
    }
}



/* USER CODE END Application */

